﻿#include "SdrRadar.h"
#include <cstring>
#include <iconv.h>
#include <memory>
#include <sstream>
#include <string>
#include "KafkaClient.h"
#include "spdlog/fmt/bundled/format.h"
#include "spdlog/spdlog.h"

void SdrRadarClient::Init(std::shared_ptr<DeviceInfo> info) {
    _device_info = info;
    _client = std::make_shared<TcpClient>();
    _client->Init(info->IP, info->Port);
    _client->SetAutoReconnect(true, 5);

    _client->SetConnectedCallback(std::bind(&SdrRadarClient::OnConnected, this));
    _client->SetRcvDataCallback(std::bind(&SdrRadarClient::OnReceiveData, this, std::placeholders::_1));
}

void SdrRadarClient::Start() {
    if (_client) {
        _client->Connect();
    }
}

void SdrRadarClient::Stop() {
    if (_client) {
        _client->Disconnect();
    }
}

/// <summary>
/// 连接上时发送初始化命令
/// </summary>
void SdrRadarClient::OnConnected() {}

void SdrRadarClient::OnReceiveData(const toolkit::Buffer::Ptr &buffer) {
    std::stringstream ss;
    for (int i = 0; i < buffer->size(); i++) {
        ss << fmt::format("{:02X} ", buffer->data()[i]);
    }
    // SPDLOG_INFO("rcv-->  " + ss.str());
    _buffer.append(buffer->data(), buffer->size());

    ParseData();
}

void SdrRadarClient::ParseData() {
    int pos = -1;
    auto len = _buffer.size();

    for (size_t i = 0; i < len - i; i++) {
        if ((uint8_t)_buffer[i] == SDR_HEAD_1 && (uint8_t)_buffer[i + 1] == SDR_HEAD_2) {
            pos = i;
            break;
        }
    }

    if (pos < 0)
        return;

    if (pos > 0)
        _buffer.erase(0, pos);

    SDR_Frame frame;
    memcpy(&frame, _buffer.c_str(), sizeof(SDR_Frame));
    _buffer.erase(0, sizeof(SDR_Frame));

    if (_buffer.length() > frame.Length) {
        if (frame.Commmand == SDR_COMMAND_HEARTBEAT) {
            SPDLOG_INFO("SDR_COMMAND_HEARTBEAT : {}", frame.Length);
        }

        if (frame.Commmand == SDR_COMMAND_OBJECT) {
            SPDLOG_INFO("SDR_COMMAND_OBJECT : {}", frame.Length);
            SPDLOG_INFO("===================================================");
            // auto objects = (SDR_Objects *)frame.Data;
            auto objects = (SDR_Objects *)_buffer.data();

            int count = objects->Count;
            SPDLOG_INFO("Frame: {}   Count: {}", objects->Frame, count);

            for (int i = 0; i < count; i++) {
                auto x = (objects->Object[i].X - 32768) / 100.0;
                auto y = objects->Object[i].Y / 20.0;

                double x_speed = 0, y_speed = 0;
                x_speed = std::abs((std::abs(objects->Object[i].VelocityX) - 32768) / 100.0);
                y_speed = std::abs((std::abs(objects->Object[i].VelocityY) - 32768) / 100.0);
                // else
                //     x_speed = (objects->Object[i].VelocityX + 32768) / 100.0;

                // if(objects->Object[i].VelocityY > 0)
                //     y_speed = (objects->Object[i].VelocityY - 32768) / 100.0;
                // else
                //     y_speed = (objects->Object[i].VelocityY + 32768) / 100.0;

                SPDLOG_INFO(
                    "Object {}: ({:5.1f}, {:5.1f}, {:5.1f}, {:5.1f})  <--->  ({:6d}, {:6d})", i, x, y, x_speed, y_speed,
                    objects->Object[i].VelocityX, objects->Object[i].VelocityY);
            }
        }

        _buffer.erase(0, frame.Length);
    }

    if (_buffer.length() > 6) {
        ParseData();
    }
}
